/**
 * @FilePath: \ros2\src\agv_control\haredware\include\hardware\hardware.hpp
 * @Date: 2025-03-13 08:09:29
 * @Description:  
 * @Author: haibo
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-20 19:45:39
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#ifndef __HARDWARE__HPP_
#define __HARDWARE__HPP_

#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/macros.hpp"
#include "encoder/encoder.h"
#include "motor/motor.h"
#include "agv_msgs/msg/driver_state.hpp"
#include "agv_msgs/msg/drivers_info.hpp"
#include "agv_msgs/msg/read_in_puts_new.hpp"
#include "agv_msgs/msg/output_port.hpp"
#include "agv_srvs/srv/motor_enable.hpp"
#include "agv_srvs/srv/set_canopen_entry.hpp"
#include "agv_srvs/srv/output_interface.hpp"

#include "std_srvs/srv/trigger.hpp"
#include "common/event_publisher.h"

constexpr int kEventParamIdInitError = 101;
constexpr int kEventParamIdRecoverError = 102;

constexpr int kMotorDevice = 0;
constexpr int kEncoderDevice = 1;    

// #define SIMULATION_MODE 

namespace hardware
{

#define AGVEvent_TO_STREAM(stream, event) \
    stream << "AGVEvent:\n"; \
    stream << "  header:\n"; \
    stream << "    stamp: " << event.header.stamp.sec << "." << std::setfill('0') << std::setw(9) << event.header.stamp.nanosec << "\n"; \
    stream << "    frame_id: " << event.header.frame_id << "\n"; \
    stream << "  type: " << static_cast<int>(event.type) << "\n"; \
    stream << "  action: " << static_cast<int>(event.action) << "\n"; \
    stream << "  ackreq: " << static_cast<int>(event.ackreq) << "\n"; \
    stream << "  eventtime: " << std::fixed << std::setprecision(3) << event.eventtime << "s\n"; \
    stream << "  vellimit: " << event.vellimit << "m/s\n"; \
    stream << "  ecode: " << event.ecode << "\n"; \
    stream << "  param1: " << event.param1 << "\n"; \
    stream << "  param2: " << event.param2 << "\n"; \
    stream << "  param3: " << event.param3 << "\n"; \
    stream << "  param4: " << event.param4 << "\n"; \
    stream << "  description_cn: \"" << event.description_cn << "\"\n"; \
    stream << "  description: \"" << event.description << "\""

inline std::ostream& operator<<(std::ostream& os, const agv_msgs::msg::AGVEvent& event) {
  AGVEvent_TO_STREAM(os, event);
  return os;
}

using HWVariant = std::variant<std::shared_ptr<ros_canopen_driver::Motor>, 
                                std::shared_ptr<encoder::Encoder>>;

struct HWDevice
{
  HWVariant device;

  std::string name;
  std::string start_seq;
  int type;
  int index;

  // rclcpp::Time read_time;
  // rclcpp::Time write_time;
  bool initialized;
  double velocity_command;
  double position_command;
  double position;
  double velocity;
  double effort;
  double timestamp;

  HWDevice(const HWVariant& dev, const std::string& dev_name, int dev_type, int dev_index = 0)
        : device(dev), name(dev_name), start_seq(""), type(dev_type), index(dev_index), initialized(false),
          velocity_command(0.0), position_command(0.0), position(0.0), velocity(0.0), effort(0.0),
          timestamp(0.0)
  {}

  void reset() {
    velocity_command = 0.0;
    position_command = 0.0;
    position = 0.0;
    velocity = 0.0;
    effort = 0.0;
    timestamp = 0.0;
  }
  
  void set_seq(const std::string seq) {
    start_seq = seq;
  }

  template<typename T>
  std::shared_ptr<T> getDevice() {
    if (std::holds_alternative<std::shared_ptr<T>>(device)) {
      return std::get<std::shared_ptr<T>>(device);
    }
    return nullptr;
  }
};

class Hardware : public rclcpp::Node
{
public:
    explicit Hardware(const std::string & node_name)
    : Node(node_name), 
    velocity_interface_running_(false),
    position_interface_running_(false),
    driver_initialized_(false),
    reset_level_(false)
    {
      RCLCPP_INFO(this->get_logger(), "Hardware node initialized.");
    }

    ~Hardware() 
    {
      if (spin_thread_.joinable()) {
        rclcpp::shutdown();
        spin_thread_.join();
      }
    }

    void setup();
    bool init_hardware(const std::string name, const int index);
    bool declare_parameters();
    void reset_device_status() { driver_status_.resize(devices_.size()); } 
    bool read();
    bool write();

    HWDevice* find_device(const std::string& name) {
      auto it = std::find_if(devices_.begin(), devices_.end(), [&name](const HWDevice& device) {
          return device.name == name;
      });
      
      if (it != devices_.end()) {
        return &(*it); 
      }
      return nullptr; 
    }
    
    bool init_single_servo(const std::string& joint_name);
    bool halt_single_servo(const std::string& joint_name);
    bool recover_single_servo(const std::string& joint_name);
    bool init_servos();
    bool recover_servos();
    bool shutdown_servos();
    bool halt_servos();
    bool has_error();
    bool call_nmt_service(uint32_t value);
    bool call_output_service();
    void quit();
private:
    // void wait_for_service();
    void state_publish();
    void hardware_timer_callback();
    bool motor_enable_callback(const std::shared_ptr<agv_srvs::srv::MotorEnable::Request> req, 
                      std::shared_ptr<agv_srvs::srv::MotorEnable::Response> res);
    bool handle_init(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                      std::shared_ptr<std_srvs::srv::Trigger::Response> res);
    bool handle_recover(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                      std::shared_ptr<std_srvs::srv::Trigger::Response> res);
    bool handle_halt(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                      std::shared_ptr<std_srvs::srv::Trigger::Response> res);
    bool handle_shutdown(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                     std::shared_ptr<std_srvs::srv::Trigger::Response> res);
   
    void reset_button_callback(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg);

    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_init_;
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_recover_;
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_halt_;
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_shutdown_;
    rclcpp::Service<agv_srvs::srv::MotorEnable>::SharedPtr srv_motor_enable_;
    
    rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr readinput_sub_;
    rclcpp::Client<agv_srvs::srv::SetCanopenEntry>::SharedPtr set_entry_client_;
    rclcpp::Client<agv_srvs::srv::OutputInterface>::SharedPtr output_client_;

    agv_msgs::msg::DriverState driver_state_;
    agv_msgs::msg::DriversInfo drivers_info_;

    std::vector<agv_msgs::msg::DriverState> driver_status_;
    std::vector<agv_msgs::msg::OutputPort> outports_;

    common::EventPublisher event_publisher_;
    std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::DriversInfo>> drivers_info_publisher_; 
   
    bool velocity_interface_running_;
    bool position_interface_running_;
    std::vector<HWDevice> devices_; 
    bool driver_initialized_;
    bool reset_level_;

    std::map<std::string, std::future<bool>>  init_futures_;
    std::thread spin_thread_;
    rclcpp::TimerBase::SharedPtr hardware_timer_;

#ifdef SIMULATION_MODE
    bool enable_motor_ = false;
#endif 
};
}  // namespace hardware

#endif  //